/**
  ****************************(C) COPYRIGHT 2019 DJI****************************
  * @file       gimbal_task.c/h
  * @brief      gimbal control task, because use the euler angle calculated by
  *             gyro sensor, range (-pi,pi), angle set-point must be in this 
  *             range.gimbal has two control mode, gyro mode and enconde mode
  *             gyro mode: use euler angle to control, encond mode: use enconde
  *             angle to control. and has some special mode:cali mode, motionless
  *             mode.
  *             完成云台控制任务，由于云台使用陀螺仪解算出的角度，其范围在（-pi,pi）
  *             故而设置目标角度均为范围，存在许多对角度计算的函数。云台主要分为2种
  *             状态，陀螺仪控制状态是利用板载陀螺仪解算的姿态角进行控制，编码器控制
  *             状态是通过电机反馈的编码值控制的校准，此外还有校准状态，停止状态等。
  * @note       
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2018     RM              1. done
  *  V1.1.0     Nov-11-2019     RM              1. add some annotation
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2019 DJI****************************
  */

#ifndef GIMBAL_TASK_H
#define GIMBAL_TASK_H
#include "struct_typedef.h"
#include "CAN_receive.h"
#include "pid.h"
#include "remote_control.h"
#include "cv_control.h"
#include  "user_lib.h"


//****************
#define DEGREESTORAD 0.01745f
//******************



//用于计算云台陀螺仪角速度的增量式PID
//enconder speed close-loop PID params, max out and max iout
//编码器速度环 PID参数以及 PID最大输出，积分输出
#if CAR
#define PITCH_GYRO_SPEED_PID_KP        12000.0f
#define PITCH_GYRO_SPEED_PID_KI        1000.0f
#define PITCH_GYRO_SPEED_PID_KD        3000.0f
#define PITCH_GYRO_SPEED_PID_MAX_OUT   30000.0f
#define PITCH_GYRO_SPEED_PID_MAX_IOUT  3000.0f
#define YAW_GYRO_SPEED_PID_KP        12000.0f 
#define YAW_GYRO_SPEED_PID_KI        500.0f
#define YAW_GYRO_SPEED_PID_KD        3000.0f
#define YAW_GYRO_SPEED_PID_MAX_OUT   30000.0f
#define YAW_GYRO_SPEED_PID_MAX_IOUT  0.0f
#else
#define PITCH_GYRO_SPEED_PID_KP        6000.0f
#define PITCH_GYRO_SPEED_PID_KI        200.0f
#define PITCH_GYRO_SPEED_PID_KD        3000.0f
#define PITCH_GYRO_SPEED_PID_MAX_OUT   30000.0f
#define PITCH_GYRO_SPEED_PID_MAX_IOUT  3000.0f
#define YAW_GYRO_SPEED_PID_KP        12000.0f 
#define YAW_GYRO_SPEED_PID_KI        500.0f
#define YAW_GYRO_SPEED_PID_KD        3000.0f
#define YAW_GYRO_SPEED_PID_MAX_OUT   30000.0f
#define YAW_GYRO_SPEED_PID_MAX_IOUT  0.0f
#endif


//用于计算云台《辅瞄》陀螺仪角速度的增量式PID
//gyro speed close-loop PID params, max out and max iout
//速度环 PID参数以及 PID最大输出，积分输出
#define PITCH_CV_GYRO_SPEED_PID_KP        2900.0f
#define PITCH_CV_GYRO_SPEED_PID_KI        60.0f
#define PITCH_CV_GYRO_SPEED_PID_KD        0.0f
#define PITCH_CV_GYRO_SPEED_PID_MAX_OUT   30000.0f
#define PITCH_CV_GYRO_SPEED_PID_MAX_IOUT  10000.0f
#define YAW_CV_GYRO_SPEED_PID_KP        14000.0f
#define YAW_CV_GYRO_SPEED_PID_KI        80.0f
#define YAW_CV_GYRO_SPEED_PID_KD        30.0f
#define YAW_CV_GYRO_SPEED_PID_MAX_OUT   30000.0f
#define YAW_CV_GYRO_SPEED_PID_MAX_IOUT  7000.0f


#define PITCH_ERROR_GYRO_SPEED_PID_KP        2900.0f
#define PITCH_ERROR_GYRO_SPEED_PID_KI        60.0f
#define PITCH_ERROR_GYRO_SPEED_PID_KD        0.0f
#define PITCH_ERROR_GYRO_SPEED_PID_MAX_OUT   30000.0f
#define PITCH_ERROR_GYRO_SPEED_PID_MAX_IOUT  10000.0f
#define YAW_ERROR_GYRO_SPEED_PID_KP        2000.0f
#define YAW_ERROR_GYRO_SPEED_PID_KI        20.0f
#define YAW_ERROR_GYRO_SPEED_PID_KD        0.0f
#define YAW_ERROR_GYRO_SPEED_PID_MAX_OUT   20000.0f
#define YAW_ERROR_GYRO_SPEED_PID_MAX_IOUT  1500.0f




//用于计算云台绝对值角度的位置式PID参数
//pitch gyro angle close-loop PID params, max out and max iout
//pitch 角度环 角度由陀螺仪解算 PID参数以及 PID最大输出，积分输出
#define PITCH_ABSOLUTE_ANGLE_PID_KP 25.0f
#define PITCH_ABSOLUTE_ANGLE_PID_KI 0.0f
#define PITCH_ABSOLUTE_ANGLE_PID_KD 0.3f
#define PITCH_ABSOLUTE_ANGLE_PID_MAX_OUT 10.0f
#define PITCH_ABSOLUTE_ANGLE_PID_MAX_IOUT 0.0f
#define YAW_ABSOLUTE_ANGLE_PID_KP        18.0f
#define YAW_ABSOLUTE_ANGLE_PID_KI        0.0f
#define YAW_ABSOLUTE_ANGLE_PID_KD        0.0f
#define YAW_ABSOLUTE_ANGLE_PID_MAX_OUT   10.0f
#define YAW_ABSOLUTE_ANGLE_PID_MAX_IOUT  0.0f


//用于计算云台绝对值角度的位置式PID参数
//pitch gyro angle close-loop PID params, max out and max iout
//pitch 角度环 角度由陀螺仪解算 PID参数以及 PID最大输出，积分输出
//#define PITCH_ABSOLUTE_ANGLE_PID_KP 288.0f
//#define PITCH_ABSOLUTE_ANGLE_PID_KI 0.0f
//#define PITCH_ABSOLUTE_ANGLE_PID_KD 0.3f
//#define PITCH_ABSOLUTE_ANGLE_PID_MAX_OUT 10.0f
//#define PITCH_ABSOLUTE_ANGLE_PID_MAX_IOUT 0.0f
//#define YAW_ABSOLUTE_ANGLE_PID_KP        12.0f
//#define YAW_ABSOLUTE_ANGLE_PID_KI        0.0f
//#define YAW_ABSOLUTE_ANGLE_PID_KD        0.5f
//#define YAW_ABSOLUTE_ANGLE_PID_MAX_OUT   10.0f
//#define YAW_ABSOLUTE_ANGLE_PID_MAX_IOUT  0.0f


//用于计算云台相对值角度的位置式PID参数
//encode angle close-loop PID params, max out and max iout
//角度环 角度由编码器 PID参数以及 PID最大输出，积分输出
#define PITCH_RELATIVE_ANGLE_PID_KP 15.0f
#define PITCH_RELATIVE_ANGLE_PID_KI 0.00f
#define PITCH_RELATIVE_ANGLE_PID_KD 0.0f
#define PITCH_RELATIVE_ANGLE_PID_MAX_OUT 10.0f
#define PITCH_RELATIVE_ANGLE_PID_MAX_IOUT 0.0f
#define YAW_RELATIVE_ANGLE_PID_KP        8.0f
#define YAW_RELATIVE_ANGLE_PID_KI        0.0f
#define YAW_RELATIVE_ANGLE_PID_KD        0.0f
#define YAW_RELATIVE_ANGLE_PID_MAX_OUT   10.0f
#define YAW_RELATIVE_ANGLE_PID_MAX_IOUT  0.0f


//用于计算云台辅瞄绝对值角度的位置式PID参数
//gyro angle close-loop PID params, max out and max iout
//角度环 角度由陀螺仪解算 PID参数以及 PID最大输出，积分输出
#define PITCH_CV_ANGLE_PID_KP 15.0f
#define PITCH_CV_ANGLE_PID_KI 0.0f
#define PITCH_CV_ANGLE_PID_KD 0.0f
#define PITCH_CV_ANGLE_PID_MAX_OUT 10.0f
#define PITCH_CV_ANGLE_PID_MAX_IOUT 0.0f
#define YAW_CV_ANGLE_PID_KP        50.0f
#define YAW_CV_ANGLE_PID_KI        0.0f
#define YAW_CV_ANGLE_PID_KD        10.0f
#define YAW_CV_ANGLE_PID_MAX_OUT   10.0f
#define YAW_CV_ANGLE_PID_MAX_IOUT  0.0f


#define PITCH_ERROR_PID_KP 10.0f
#define PITCH_ERROR_PID_KI 0.0f
#define PITCH_ERROR_PID_KD 0.5f
#define PITCH_ERROR_PID_MAX_OUT 10.0f
#define PITCH_ERROR_PID_MAX_IOUT 0.0f
#define YAW_ERROR_PID_KP        26.0f
#define YAW_ERROR_PID_KI        0.0f
#define YAW_ERROR_PID_KD        0.3f
#define YAW_ERROR_PID_MAX_OUT   10.0f
#define YAW_ERROR_PID_MAX_IOUT  0.0f



//任务初始化 空闲一段时间
#define GIMBAL_TASK_INIT_TIME 201

//yaw控制通道
#define YAW_CHANNEL   2       //
//pitch控制通道
#define PITCH_CHANNEL 3       //
//状态开关通道
#define GIMBAL_MODE_CHANNEL 0 //遥控器右面挡位
//turn 180°
//掉头180 按键
#define TURN_KEYBOARD KEY_PRESSED_OFFSET_F
//turn speed
//掉头云台速度
#define TURN_SPEED    0.04f
//按键宏定义区
#define TEST_KEYBOARD KEY_PRESSED_OFFSET_R

//rocker value deadband
//遥控器输入死区，因为遥控器存在差异，摇杆在中间，其值不一定为零
#define RC_DEADBAND   10

//摇杆灵敏度
// #define YAW_RC_SEN    -(0.4)/660
// #define PITCH_RC_SEN  -(0.1)/660
#define YAW_RC_SEN -(0.01)/660
#define PITCH_RC_SEN -(0.005)/660

//辅助瞄准控制下，遥控器微调角度的灵敏度
#define YAW_RC_CV_SEN    -(0.005)/660
#define PITCH_RC_CV_SEN  -(0.0025)/660  //0.005
//鼠标灵敏度
#define YAW_MOUSE_SEN   0.00002f
#define PITCH_MOUSE_SEN 0.00001f


//视觉误差增量置信度
#define YAW_CV_SEN    1.0f
#define PITCH_CV_SEN  1.0f


#define  YAW_GYRO_SEN   -(10.0)/660
#define PITCH_GYRO_SEN  -(0.0)/660

//
#define YAW_ENCONDER_SEN    0.01f
#define PITCH_ENCONDER_SEN  0.01f

//云台任务控制周期
#define GIMBAL_CONTROL_TIME 2
#define GIMBAL_CONTROL_TIME_S 0.002
#define GIMBAL_CONTROL_FREQUENCY 500

//test mode, 0 close, 1 open
//云台测试模式 宏定义 0 为不使用测试模式
#define GIMBAL_TEST_MODE 0


//pitch反装，yaw不反装
#define PITCH_TURN  1
#define YAW_TURN    0

//电机码盘值最大以及中值
#define HALF_ECD_RANGE  4096
#define ECD_RANGE       8191





//云台初始化回中值，允许的误差,并且在误差范围内停止一段时间以及最大时间6s后解除初始化状态，
#define GIMBAL_INIT_ANGLE_ERROR     0.1f
#define GIMBAL_INIT_STOP_TIME       100
#define GIMBAL_INIT_TIME            6000
#define GIMBAL_CALI_REDUNDANT_ANGLE 0.15f

//云台初始化回中值的速度以及控制到的角度
#define GIMBAL_INIT_PITCH_SPEED     0.008f
#define GIMBAL_INIT_YAW_SPEED       0.008f

#define INIT_YAW_SET    0.0f
#define INIT_PITCH_SET  0.0f

//云台校准中值的时候，发送原始电流值，以及堵转时间，通过陀螺仪判断堵转
#define GIMBAL_CALI_MOTOR_SET   8000
#define GIMBAL_CALI_STEP_TIME   2000
#define GIMBAL_CALI_GYRO_LIMIT  0.1f

#define GIMBAL_CALI_PITCH_MAX_STEP  1
#define GIMBAL_CALI_PITCH_MIN_STEP  2
#define GIMBAL_CALI_YAW_MAX_STEP    3
#define GIMBAL_CALI_YAW_MIN_STEP    4

#define GIMBAL_CALI_START_STEP  GIMBAL_CALI_PITCH_MAX_STEP
#define GIMBAL_CALI_END_STEP    5

//判断遥控器无输入的时间以及遥控器无输入判断，设置云台yaw回中值以防陀螺仪漂移
#define GIMBAL_MOTIONLESS_RC_DEADLINE 10
#define GIMBAL_MOTIONLESS_TIME_MAX    3000

//电机编码值转化成角度值
#ifndef MOTOR_ECD_TO_RAD
#define MOTOR_ECD_TO_RAD 0.000766990394f //      2*  PI  /8192
#endif

#define DEGREESTORAD 0.01745f

#define QIANKUI -3000.0f

typedef enum
{
    GIMBAL_MOTOR_RAW = 0, //电机原始值控制
    GIMBAL_MOTOR_GYRO,    //电机陀螺仪角度控制
    GIMBAL_MOTOR_ENCONDE, //电机编码值角度控制  
		GIMBAL_MOTOR_CV,			//电机CV控制模式（使用增量，绝对角度增量）
    GIMBAL_MOTOR_ERROR,
	  GIMBAL_MOTOR_ZUNI,
} gimbal_motor_mode_e;

//云台独立使用Pid结构体
typedef struct
{
    fp32 kp;
    fp32 ki;
    fp32 kd;

    fp32 set;
    fp32 get;
    fp32 err;

    fp32 max_out;
    fp32 max_iout;

    fp32 Pout;
    fp32 Iout;
    fp32 Dout;

    fp32 out;
} gimbal_PID_t;

//云台电机结构体
typedef struct
{ 
  	//当前电机的模式，和上一次电机的模式的枚举类型
    gimbal_motor_mode_e gimbal_motor_mode;
    gimbal_motor_mode_e last_gimbal_motor_mode; 
    
    //电机反馈参数结构体
    const motor_measure_t *gimbal_motor_measure;
    
    //绝对角度控制下的PID结构体
    gimbal_PID_t gimbal_motor_absolute_angle_pid;   
    pid_type_def gimbal_motor_absolute_gyro_speed_pid; 
    
    //相对角度控制下的PID结构体
    gimbal_PID_t gimbal_motor_relative_angle_pid;
    pid_type_def gimbal_motor_relative_enconde_speed_pid;
    
    //绝对角度下，视觉CV控制PID结构体(使用增量×一定系数得到短时间内的增量)
	  gimbal_PID_t gimbal_motor_CV_angle_pid;
	  pid_type_def gimbal_motor_CV_gyro_speed_pid;

    //绝对角度下，视觉ERROR控制PID结构体(使用绝对角度误差量)
    pid_type_def gimbal_motor_error_pid;
    pid_type_def gimbal_motor_error_gyro_speed_pid;
	  
    //校准使用 中值编码值 最大的编码值，最小的编码值
    uint16_t offset_ecd;     //ecd
	  fp32 max_relative_angle; //rad
    fp32 min_relative_angle; //rad

    fp32 max_absolute_angle; //rad
    fp32 min_absolute_angle; //rad
    
    //当前相对角度和目标相对角度
    fp32 relative_angle;     //rad
    fp32 relative_angle_set; //rad
    
    //当前绝对角度和目标绝对角度
    fp32 absolute_angle;     //rad
    fp32 absolute_angle_set; //rad
    
    //当前陀螺仪测得的绝对角速度和绝对角速度设定
	  fp32 motor_gyro;         //rad/s   主要用这个
    fp32 motor_gyro_set;     //rad/s

    //当前通过电机反馈测得的ECD角速度ECD角速度设定
	  fp32 motor_enconde_speed;    //ecd/s
    fp32 motor_enconde_speed_set;//ecd/s
    
    //当前视觉误差，和设定误差
    fp32 cv_error;
    fp32 cv_error_set;
		
	  //直驱模式下发送的电流		
	  fp32 raw_cmd_current;
    //PID算出的实时电流设定值
    fp32 current_set;
    //最后赋值的电流
    int16_t given_current;
} gimbal_motor_t;//云台电机结构体

//校准使用的结构体
typedef struct
{
    fp32 max_yaw;
    fp32 min_yaw;
    fp32 max_pitch;
    fp32 min_pitch;
    uint16_t max_yaw_ecd;
    uint16_t min_yaw_ecd;
    uint16_t max_pitch_ecd;
    uint16_t min_pitch_ecd;
    uint8_t step;
} gimbal_step_cali_t;

//云台控制总结构体
typedef struct
{
    const RC_ctrl_t *gimbal_rc_ctrl;

    //云台角度指针
    const fp32 *gimbal_INT_angle_point;
    //云台角速度指针
    const fp32 *gimbal_INT_gyro_point;
		//云台角度指针
		const fp32 *gimbal_INT_accel_point;
	
  	CV_Ctrl_t *cv_ctrl;
    gimbal_motor_t gimbal_yaw_motor;//yaw轴云台电机结构体
    gimbal_motor_t gimbal_pitch_motor;//pitch轴云台电机结构体
    gimbal_step_cali_t gimbal_cali;
	  uint16_t car_id;
	
} gimbal_control_t;//云台控制总结构体




/**
  * @brief          返回yaw 电机数据指针
  * @param[in]      none
  * @retval         yaw电机指针
  */
extern const gimbal_motor_t *get_yaw_motor_point(void);

/**
  * @brief          返回pitch 电机数据指针
  * @param[in]      none
  * @retval         pitch
  */
extern const gimbal_motor_t *get_pitch_motor_point(void);

/**
  * @brief          云台任务，间隔 GIMBAL_CONTROL_TIME 1ms
  * @param[in]      pvParameters: 空
  * @retval         none
  */
extern void gimbal_task(void const *pvParameters);
/*
  * @brief          云台校准计算，将校准记录的中值,最大 最小值返回
  * @param[out]     yaw 中值 指针
  * @param[out]     pitch 中值 指针
  * @param[out]     yaw 最大相对角度 指针
  * @param[out]     yaw 最小相对角度 指针
  * @param[out]     pitch 最大相对角度 指针
  * @param[out]     pitch 最小相对角度 指针
  * @retval         返回1 代表成功校准完毕， 返回0 代表未校准完
  * @waring         这个函数使用到gimbal_control 静态变量导致函数不适用以上通用指针复用
  */
extern bool_t cmd_cali_gimbal_hook(uint16_t *yaw_offset, uint16_t *pitch_offset, fp32 *max_yaw, fp32 *min_yaw, fp32 *max_pitch, fp32 *min_pitch);

/**
  * @brief          云台校准设置，将校准的云台中值以及最小最大机械相对角度
  * @param[in]      yaw_offse:yaw 中值
  * @param[in]      pitch_offset:pitch 中值
  * @param[in]      max_yaw:max_yaw:yaw 最大相对角度
  * @param[in]      min_yaw:yaw 最小相对角度
  * @param[in]      max_yaw:pitch 最大相对角度
  * @param[in]      min_yaw:pitch 最小相对角度
  * @retval         返回空
  * @waring         这个函数使用到gimbal_control 静态变量导致函数不适用以上通用指针复用
  */
extern void set_cali_gimbal_hook(const uint16_t yaw_offset, const uint16_t pitch_offset, const fp32 max_yaw, const fp32 min_yaw, const fp32 max_pitch, const fp32 min_pitch);
#endif

extern void get_relative_angle(float *relative_angle);
